% fc_opt_stance

function [torque_vector, output, J_opt, J_fsolve, torque_fsolve] = fc_opt_stance()

clear('all');
close('all');
addpath('../animations', '../controllers', '../dynamics', '../environments', '../library');

fp_options = optimset('Display','iter','TolFun',1e-15, 'TolX',1e-10);

tic
td_angle = fsolve(@fp_fun,  evalin('base','first_guess'), fp_options);
error = fp_fun(td_angle)
toc

assignin('base','td_angle',td_angle);

% anim = AnimateForceControlModelClass(sim.gnd, output);
% anim.animate;

close all



A = [];
B = [];
Aeq = [];
Beq = [];
lb = [];
ub = [];

torque_vector = evalin('base','initial_torque_vector'); %6*ones(evalin('base','NUM_DISCRETE_TORQUES'),1);

to_options = optimset('Display','iter','TolFun',1e-15, 'TolX',1e-10);


% x = fsolve(@basiccon,  torque_vector, to_options);
x = torque_vector;

% torque_fsolve = x;
J_fsolve = obj_fun(x)

% pause

opt_options = optimset('Display','iter','Algorithm','sqp');

[torque_vector, J_opt] = fmincon(@obj_fun, x, A, B, Aeq, Beq, lb, ub, @nonlcon, opt_options);

[output, sim] = singlestepsim(torque_vector);


% figure;
% title('CoM Height');
% xlabel('Time (s)');
% ylabel('Height (m)');
% hold on;
% grid on
% for i = 1 : phases
%     plot(output{i, 1}, output{i, 2}(:, sim.yci), 'linewidth', 3);
% end
%
% figure;
% title('CoM Velocity');
% xlabel('Time (s)');
% ylabel('Velocity (m/s)');
% hold on;
% grid on
% for i = 1 : phases
%     plot(output{i, 1}, output{i, 2}(:, sim.dxci), 'linewidth', 3);
% end
%
%
% figure;
% title('Gear Angle');
% xlabel('Time (s)');
% ylabel('Angle');
% hold on;
% grid on
% for i = 1 : phases
%     plot(output{i, 1}, output{i, 2}(:, sim.oli), 'linewidth', 3);
% end
%
% close all

% anim = AnimateForceControlModelClass(sim.gnd, output);
%         anim.animate;



%% Fixed Point function

    function error = fp_fun(x)
        
        phases = 3;
        output = cell(phases, 2);
        
        x0 = [ 0, evalin('base','x_dot'), evalin('base','y'), 0, 0, 0, x, 0, ForceControlModelClass.r0 * ForceControlModelClass.G / ForceControlModelClass.rl, 0, 0];
        
        % controller = ComControllerClass;
        controller = SimpleControllerClass;
        
        sim = ForceControlModelClass(x0, @flat_rigid_environment, controller);
        
        for i = 1 : phases
            [output{i, 1}, output{i, 2}] = sim.run();
        end
        
        error = [   output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci);    % horizontal velocity
            output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci)       ];      % COM height
        
        % anim = AnimateForceControlModelClass(sim.gnd, output);
        % anim.animate;
        
    end



%% SingleStepSim

    function [output, sim] = singlestepsim(x)
        
        
        
        phases = 3;
        output = cell(phases, 2);
        
        x0 = [ 0, evalin('base','x_dot'), evalin('base','y'), 0, 0, 0, evalin('base','td_angle'), 0, ForceControlModelClass.r0 * ForceControlModelClass.G / ForceControlModelClass.rl, 0, 0];
        
        controller = OptTorqueControllerClass;
        
        controller.time_vector = linspace(0,evalin('base','MAX_STANCE_TIME'),evalin('base','NUM_DISCRETE_TORQUES'));
        controller.torque_vector = x;
        
        sim = ForceControlModelClass(x0, @dissipative_environment, controller);
        
        for i = 1 : phases
            [output{i, 1}, output{i, 2}] = sim.run();
        end
        
        %         close all
        
                figure(1)
                plot(controller.time_vector, controller.torque_vector,'bx-')
                pause(0.05)
        
                figure(2)
                anim = AnimateForceControlModelClass(sim.gnd, output);
                anim.animate;
        
    end

%% Objective Function

    function J = obj_fun(x)
        
        try
            
            [output, sim] = singlestepsim(x);
            
            t = output{2,1};
            x = output{2,2};
            
            
            r = sqrt( ( x(:,sim.yci) - x(:,sim.yti) ).^2 + ...
                ( x(:,sim.xci) - x(:,sim.xti) ).^2 );
            
            power = sim.kl .* r .* sim.rl .* x(:,sim.doli) ./ sim.G ...
                - sim.kl .* sim.rl^2 .* x(:,sim.oli) .* x(:,sim.doli) ./ sim.G^2;
            
            % Operation to ignore all negative work
%             power = (power < 0).*power;
            
            


            smooth_eps = 0.01;
            smoothed_power = sqrt(power.^2 + e^2);
            
            
            J = trapz(t, abs(power));
            
            %         clf
            %         plot(t, x(:,sim.oli))
            
            J
        catch
            J = realmax
        end
        
    end

%% Nonlinear Constraint Function

    function [C, Ceq] = nonlcon(x)
        
        try
            [output, sim] = singlestepsim(x);
            
            C = [0; 0];
            
%             Ceq = [   sign((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci)))*sqrt(sqrt(abs((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci)))));
%                 sign(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))*sqrt(sqrt(abs(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))))       ];
            
            Ceq = [   output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci);
                output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci)];
            
            Ceq_debug = [   output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci);
                (output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))       ]
            
            
        catch
            
            C = [0; 0];
            Ceq = [realmax; realmax]
        end
        
    end



%% Basic Equilibrium Constraint

    function [Ceq] = basiccon(x)
        
        try
            [output, sim] = singlestepsim(x);
            
            %         C = [0; 0];
%             Ceq = [   sign((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci)))*sqrt(sqrt(abs((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci)))));
%                 sign(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))*sqrt(sqrt(abs(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))))       ];
            
            Ceq = [   sign((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci)))*(abs((output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci))))^2;
                sign(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))*(abs(output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci)))^1       ];
            
            Ceq_debug = [   output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci);
                (output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci))       ]
        catch
            Ceq = [realmax; realmax]
        end
        
    end

end